#include <defs.h>
#include <pq.h>
#include <bwio.h>
#include <syscalls.h>
#include <string.h>
#include <vic.h>
#include <uart.h>

int Min(int a, int b) {
	return (a < b ? a : b);
}

int SysCreate( sched *s, td *task, int priority, void (*func) (), char *name) {
  // check for error conditions
  if ( priority < 0 || priority >= MAX_PRI ) {    
    SysPanic(s, task, "SysCreate", "Invalid priority level.");
  }
  if ( s->next_task_id == MAX_TASKS ) {
    SysPanic(s, task, "SysCreate", "The number of tasks has reached the limit.");
  }

  // set up the task descriptor
  // NOTE: initialize everything - Resource Acquisition Is Initialization!
  int id = s->next_task_id++;
  s->tasks[id].id = id;
  s->tasks[id].sp = 0x01e00000 - STACK_SIZE * id;
  s->tasks[id].spsr = 0x600000d0;
  s->tasks[id].ret = 0xdeadbeef;
  s->tasks[id].mode = SWI;
  s->tasks[id].parent = task;
  s->tasks[id].pri = priority;
  s->tasks[id].state = STATE_READY;
  s->tasks[id].next = NULL;
  s->tasks[id].prev = NULL;
  PqInit( &(s->tasks[id].sendQ) );
  s->tasks[id].msg = NULL;
  s->tasks[id].msg_len = -1;
  s->tasks[id].reply = NULL;
  s->tasks[id].reply_len = -1;
  s->tasks[id].sender_tid = NULL;
  s->tasks[id].event = NULL;
  s->tasks[id].event_len = -1;
  
  StringCopy(s->tasks[id].name, name);

  // set up the task stack
  int i;
  for (i = 13; i >= 0; i--) {                               // r12-r0
    TdPushRegister( &(s->tasks[id]), 0xdeadbeef );
  }
  TdPushRegister( &(s->tasks[id]), (int)( func ) + CODE_BASE );  // lr

  // place into priority queue
  PqPushBack( &(s->queues[priority]), &(s->tasks[id]) );
  
  return id;
}

int SysMyTid( sched *s, td *task ) {
  return task->id;
}

int SysMyParentTid( sched *s, td *task ) {
  if ( task->parent == NULL ) return -1;
  return task->parent->id;
}

void SysPass( sched *s, td *task ) {}

void SysExit( sched *s, td *task ) {
  // notify all tasks on send queue
  while ( !PqEmpty( &task->sendQ ) ) {
    td *sender = PqPopFront( &task->sendQ );
    
    // warn the sender that the transaction is incomplete
    sender->ret = ERROR_TX_INCOMPLETE;

    // ready the sender
    sender->state = STATE_READY;
	  PqPushBack(&s->queues[sender->pri], sender);
  }
  task->state = STATE_DEFUNCT;
}

void SysFingerOfDeath( sched *s, td *task ) {
  // remove all tasks from all priority queues
  int i;
  td *next_task;
  for (i = MAX_PRI; i >= 0; i--) {
    while (!PqEmpty(&(s->queues[i]))) {
      next_task = PqPopFront(&(s->queues[i]));
      next_task->state = STATE_DEFUNCT;
    }
  }
  // finally, kill the calling task
  task->state = STATE_DEFUNCT;
}

int SysSend( sched *s, td *task, int tid, char *msg, int msg_len, char *reply, int reply_len ) {	
  // if (task->id == 9) {
  //  bwprintf(COM2, "Send: tid = %d, msg = %d, reply = %d\r\n", tid, msg_len, reply_len);
  // }
  // error checking
  if ((msg == NULL && msg_len > 0) || (reply == NULL && reply_len > 0)) {
    SysPanic(s, task, "SysSend", "Null message or reply pointer passed.");
		return ERROR_NULL_POINTER;
	}
	if (msg_len < 0 || reply_len < 0) {
    SysPanic(s, task, "SysSend", "Negative message or reply length.");
		return ERROR_INVALID_MSG_LENGTH;
  }

  if (tid < 0 || tid >= s->next_task_id) {
    SysPanic(s, task, "SysSend", "Invalid TID.");
		return ERROR_INVALID_TID;
  }

  td *sender = task;
  td *receiver = &s->tasks[tid];
	
  if (receiver->state == STATE_DEFUNCT) {
    SysPanic(s, task, "SysSend", "The state of the receiver is DEFUNCT: Transaction incomplete.");
		return ERROR_TX_INCOMPLETE;
	}
	
  // set fields so that Reply() can copy the reply
  sender->reply = reply;
	sender->reply_len = reply_len;
		
  // set fields so that Receive() can copy the sent message
	sender->msg = msg;
	sender->msg_len = msg_len;

  // can't overflow either buffer
	int to_copy = Min(receiver->msg_len, msg_len);
	
	if (receiver->state == STATE_SEND_BLOCK) {
    // the receiver has called Receive() and has an empty send queue
		// copy the sent message to the receiver
		MemCopy(receiver->msg, msg, to_copy);
		
		// ready the receiver, set appropriate return value
		*(receiver->sender_tid) = sender->id;
		//if (sender->id == 9) {
    //  bwprintf(COM2, "Receive: tid = %d, msg = %d\r\n", receiver->id, receiver->msg_len);
    //}
		
    if (msg_len > receiver->msg_len) {
      SysPanic(s, task, "SysSend", "The sender's message size greater than receiver expected.");
      receiver->ret = ERROR_MSG_TRUNCATED;
    }
    else
      receiver->ret = msg_len;
      
		receiver->state = STATE_READY;		
		PqPushBack(&s->queues[receiver->pri], receiver);
		
		// block the sender, waiting for Reply()
		sender->state = STATE_REPLY_BLOCK;		
	} 
	else {
    // block the sender, waiting for Receive()
	  sender->state = STATE_RECEIVE_BLOCK;
	  
    // the receiver has never called Receive() or has a non-empty send queue
    // put the sender on the receiver's send queue
		PqPushBack(&receiver->sendQ, sender);
	}
	
  // return for now; Reply() fills in the actual return value
  return SUCCESS;
}

int SysReceive(sched *s, td *task, int *tid, char *msg, int len) {	
	// error checking
  if (tid == NULL || (msg == NULL && len > 0)) {  
    SysPanic(s, task, "SysReceive", "Null message or tid pointer passed.");
		return ERROR_NULL_POINTER;
	}
	if (len < 0) {
    SysPanic(s, task, "SysReceive", "Negative message length.");
		return ERROR_INVALID_MSG_LENGTH;
	}

  td *receiver = task;
  
  // set fields so that Send() can copy the sent message and set tid
	receiver->msg = msg;
	receiver->msg_len = len;
  receiver->sender_tid = tid;

	if (PqEmpty(&task->sendQ)) {
    // the receiver has an empty send queue
		
    // block the receiver, waiting for Send()
		receiver->state = STATE_SEND_BLOCK;
    
    // return for now; Send() fills in the actual return value
    return SUCCESS;
	} else {
    // the receiver has a non-empty send queue
		// pop the next sender off the send queue
		td *sender = PqPopFront(&receiver->sendQ);
		*tid = sender->id;
    
    //if (*tid == 9) {
    //  bwprintf(COM2, "Receive: tid = %d, msg = %d\r\n", task->id, len);
    //}
    
		// copy the sent message to the receiver
		int to_copy = Min(sender->msg_len, len);
		MemCopy(msg, sender->msg, to_copy);
		
		// block the sender, waiting for Reply()
		sender->state = STATE_REPLY_BLOCK;

    if (sender->msg_len > len) {
      SysPanic(s, task, "SysReceive", "The sender's message size greater than receiver expected.");
      return ERROR_MSG_TRUNCATED;
    }
    else
      return sender->msg_len;
	}
}

int SysReply(sched *s, td *task, int tid, char *reply, int len) {
  //if (tid == 9) {
  //  bwprintf(COM2, "Reply: tid = %d, reply = %d\r\n", task->id, len);
  //}
  
	// error checking
	if (reply == NULL && len > 0) {
    SysPanic(s, task, "SysReply", "Null reply pointer passed, when non-empty reply expected.");
    return ERROR_NULL_POINTER;
  }
  if (len < 0) {
    SysPanic(s, task, "SysReply", "Negative reply length.");
    return ERROR_INVALID_MSG_LENGTH;
  }
	
  if (tid < 0 || tid >= s->next_task_id) {
    SysPanic(s, task, "SysReply", "Invalid TID.");
    return ERROR_INVALID_TID;
  }
		
	td *sender = &s->tasks[tid];

  if ( sender->state != STATE_REPLY_BLOCK ) {
    SysPanic(s, task, "SysReply", "Sender not reply-blocked.");
    return ERROR_NOT_REPLY_BLOCK;
  }
	
	// wake up the sender and add to ready queue
	sender->state = STATE_READY;
	PqPushBack(&s->queues[sender->pri], sender);
	
  if (len > sender->reply_len) {
    // not enough space to copy reply to sender
    sender->ret = ERROR_TX_INCOMPLETE;
    
    SysPanic(s, task, "SysReply", "Not enough space for the reply in sender.");
    return ERROR_NOT_ENOUGH_SPACE;
  } else {
    // copy the reply to sender
	  MemCopy(sender->reply, reply, len);

    sender->ret = len;
    return SUCCESS;
  }	
}

int SysAwaitEvent(sched *s, td *task, int eventid, char *event, int eventlen) {
  if (event == NULL && eventlen > 0) {
    SysPanic(s, task, "SysAwaitEvent", "Null event pointer passed, when non-empty event expected.");
    return ERROR_NULL_POINTER;
  }
  if (eventlen < 0) {
    SysPanic(s, task, "SysAwaitEvent", "Negative event length.");
    return ERROR_INVALID_MSG_LENGTH;
  }
  if (eventid <= EVENT_NONE || eventid >= EVENT_MAX) {
    SysPanic(s, task, "SysAwaitEvent", "Invalid event ID.");
    return ERROR_INVALID_EVENTID;
  }

  s->waiters[eventid] = task->id;
  task->event = event;
  task->event_len = eventlen;
  task->state = STATE_EVENT_BLOCK;
  
  return SUCCESS;
}

void SysPanic(sched *s, td *task, char *fun_name, char *msg) {
  vic *const vic2 = (vic*)( VIC2_BASE );
  int status = vic2->irq_status;
  vic2->int_en_clear = 0xf97865fb;
  uart *const uart2 = (uart*)( UART2_BASE );
  uart2->ctrl &= ~FEN_MASK;
  
  // bwprintf(COM2, "\Ec\E[2J");
  bwprintf(COM2, "EXCEPTION\r\n---------\r\n\r\nThe system has panicked inside %s.\r\n", fun_name);
  bwprintf(COM2, msg);
  
  bwprintf(COM2, "\r\n\r\nTask state dump:\r\n");
  bwprintf(COM2, "task @ 0x%x\r\n", task );
  bwprintf(COM2, "[id] %d\r\n", task->id );
  bwprintf(COM2, "[name] %s\r\n", task->name );
  bwprintf(COM2, "[sp] 0x%x\r\n", task->sp );
  bwprintf(COM2, "[spsr] 0x%x\r\n", task->spsr );
  bwprintf(COM2, "[ret] %d\r\n", task->ret );
  bwprintf(COM2, "[mode] %d\r\n", task->mode );
  // bwprintf(COM2, "[parent] %d\r\n", task->parent );
  // bwprintf(COM2, "[pri] %d\r\n", task->pri );
  // bwprintf(COM2, "[state] %d\r\n", task->state );
  bwprintf(COM2, "[msg] %x\r\n", task->msg );
  bwprintf(COM2, "[msg_len] %d\r\n", task->msg_len );
  bwprintf(COM2, "[reply] %x\r\n", task->reply );
  bwprintf(COM2, "[reply_len] %d\r\n", task->reply_len );
  // bwprintf(COM2, "[sender_tid] %d\r\n", *(task->sender_tid) );
  // bwprintf(COM2, "[event] %x\r\n", task->event );
  // bwprintf(COM2, "[event_len] %d\r\n", task->event_len );
  bwprintf(COM2, "[tag] %d\r\n", task->tag );
  
  
  bwprintf(COM2, "\r\nTask stack dump:\r\n");
  int i;
  for (i = 0; i < 15; i++)
    bwprintf(COM2, "[r%d] %x\r\n", i, *(int*)(task->sp + i * 4));
  
  
  bwprintf(COM2, "\r\nLast request dump:\r\n");
  bwprintf(COM2, "[call] %d\r\n", task->request.call);
  bwprintf(COM2, "[a0] %d\r\n", task->request.a0);
  bwprintf(COM2, "[a1] %d\r\n", task->request.a1);
  bwprintf(COM2, "[a2] %d\r\n", task->request.a2);
  bwprintf(COM2, "[a3] %d\r\n", task->request.a3);
  bwprintf(COM2, "[a4] %d\r\n", task->request.a4);
  
  bwprintf(COM2, "\r\nCall-specific information:\r\n");
  switch (task->request.call) {
    case SYSCALL_REPLY:
      bwprintf(COM2, "reply-target\r\n");
      bwprintf(COM2, "[reply] %x\r\n", s->tasks[task->request.a0].reply);
      bwprintf(COM2, "[reply_len] %d\r\n", s->tasks[task->request.a0].reply_len);
      break;
  }
  
  bwprintf(COM2, "\r\nTask state dump:\r\n");
  // int i;
  for (i = 0; i < s->next_task_id; i++) {
    bwprintf(COM2, "[%d] %s : state == %d\r\n",
        s->tasks[i].id, s->tasks[i].name, s->tasks[i].state);
  }

  bwprintf(COM2, "\r\nInterrupts asserted:\r\n");
  if (status & TIMER3_IRQ) {
    bwprintf(COM2, "Timer IRQ\r\n");
  }
  if (status & UART1_IRQ) {
    bwprintf(COM2, "UART1 IRQ\r\n");
  }
  if (status & UART2_IRQ) {
    bwprintf(COM2, "UART2 IRQ\r\n");
  }
  
  SysFingerOfDeath(s, task);
}

int SysParamTest(sched *s, td *task, int a, int b, int c, int d, int e) {
  if (a != 0x12 || b != 0x34 || c != 0x56 || d != 0x78 || e != 0x9A) {
    SysPanic(s, task, "SysParamTest", "WRONG PARAMS!!!!!");
  }
  return 0xFF;
}

